////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                                                            //
//                                                         WNProject                                                          //
//                                                                                                                            //
//            This file is distributed under the BSD 2-Clause open source license. See License.txt for details.               //
//                                                                                                                            //
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include "WNMemory/inc/WNMemory.h"
#include "WNMath/inc/WNQuaternion.h"
#include "WNMath/inc/WNBasic.h"
#include "WNMath/inc/WNMatrix33.h"
#include "WNMath/inc/WNMatrix44.h"
#include "WNMath/inc/WNVector3.h"

#include <float.h>

namespace WNMath {
    WNQuaternion operator * (WN_FLOAT32 _f, const WNQuaternion& _quaternion) {
        return(WNQuaternion(_f * _quaternion.x,
                            _f * _quaternion.y,
                            _f * _quaternion.z,
                            _f * _quaternion.w));
    }

    WNQuaternion operator / (WN_FLOAT32 _f, const WNQuaternion& _quaternion) {
        const WN_FLOAT32 fInv = 1.0f / _f;

        return(WNQuaternion(fInv * _quaternion.x,
                            fInv * _quaternion.y,
                            fInv * _quaternion.z,
                            fInv * _quaternion.w));
    }
}

using namespace WNMath;
using namespace WNMemory;

// Constructors ////////////////////////////////////////////////////////////////////////////////////

WNQuaternion::WNQuaternion() :
    x(0.0f),
    y(0.0f),
    z(0.0f),
    w(0.0f) {
}

WNQuaternion::WNQuaternion(const WN_FLOAT32* _f) :
    x(_f[0]),
    y(_f[1]),
    z(_f[2]),
    w(_f[3]) {
}

WNQuaternion::WNQuaternion(WN_FLOAT32 _x, WN_FLOAT32 _y, WN_FLOAT32 _z, WN_FLOAT32 _w) :
    x(_x),
    y(_y),
    z(_z),
    w(_w) {
}

// Operators ///////////////////////////////////////////////////////////////////////////////////////

WNQuaternion::operator WN_FLOAT32* () {
    return(&x);
}

WNQuaternion::operator const WN_FLOAT32* () const {
    return(&x);
}

WNQuaternion& WNQuaternion::operator += (const WNQuaternion& _quaternion) {
    x += _quaternion.x;
    y += _quaternion.y;
    z += _quaternion.z;
    w += _quaternion.w;

    return(*this);
}

WNQuaternion& WNQuaternion::operator -= (const WNQuaternion& _quaternion) {
    x -= _quaternion.x;
    y -= _quaternion.y;
    z -= _quaternion.z;
    w -= _quaternion.w;

    return(*this);
}

WNQuaternion& WNQuaternion::operator *= (const WNQuaternion& _quaternion) {
    const WNQuaternion quaternion = *this;

    x = quaternion.w * _quaternion.x + quaternion.x * _quaternion.w +
        quaternion.y * _quaternion.z - quaternion.z * _quaternion.y;
    y = quaternion.w * _quaternion.y + quaternion.y * _quaternion.w +
        quaternion.z * _quaternion.x - quaternion.x * _quaternion.z;
    z = quaternion.w * _quaternion.z + quaternion.z * _quaternion.w +
        quaternion.x * _quaternion.y - quaternion.y * _quaternion.x;
    w = quaternion.w * _quaternion.w - quaternion.x * _quaternion.x -
        quaternion.y * _quaternion.y - quaternion.z * _quaternion.z;

    return(*this);
}

WNQuaternion& WNQuaternion::operator *= (WN_FLOAT32 _f) {
    x *= _f;
    y *= _f;
    z *= _f;
    w *= _f;

    return(*this);
}

WNQuaternion& WNQuaternion::operator /= (WN_FLOAT32 _f) {
    const WN_FLOAT32 fInv = 1.0f / _f;

    x *= fInv;
    y *= fInv;
    z *= fInv;
    w *= fInv;

    return(*this);
}

WNQuaternion WNQuaternion::operator + () const {
    return(*this);
}

WNQuaternion WNQuaternion::operator - () const {
    return(WNQuaternion(-x, -y, -z, -w));
}

WNQuaternion WNQuaternion::operator + (const WNQuaternion& _quaternion) const {
    return(WNQuaternion(x + _quaternion.x,
                        y + _quaternion.y,
                        z + _quaternion.z,
                        w + _quaternion.w));
}

WNQuaternion WNQuaternion::operator - (const WNQuaternion& _quaternion) const {
    return(WNQuaternion(x - _quaternion.x,
                        y - _quaternion.y,
                        z - _quaternion.z,
                        w - _quaternion.w));
}

WNQuaternion WNQuaternion::operator * (const WNQuaternion& _quaternion) const {
    return(WNQuaternion(w * _quaternion.x + x * _quaternion.w +
                        y * _quaternion.z - z * _quaternion.y,
                        w * _quaternion.y + y * _quaternion.w +
                        z * _quaternion.x - x * _quaternion.z,
                        w * _quaternion.z + z * _quaternion.w +
                        x * _quaternion.y - y * _quaternion.x,
                        w * _quaternion.w - x * _quaternion.x -
                        y * _quaternion.y - z * _quaternion.z));
}

WNQuaternion WNQuaternion::operator * (WN_FLOAT32 _f) const {
    return(WNQuaternion(x * _f, y * _f, z * _f, w * _f));
}

WNQuaternion WNQuaternion::operator / (WN_FLOAT32 _f) const {
    const WN_FLOAT32 fInv = 1.0f / _f;

    return(WNQuaternion(x * fInv, y * fInv, z * fInv, w * fInv));
}

WN_BOOL WNQuaternion::operator == (const WNQuaternion& _quaternion) const {
    return(0 == WNMemCmp(this, &_quaternion, sizeof(WNQuaternion)));
}

WN_BOOL WNQuaternion::operator != (const WNQuaternion& _quaternion) const {
    return(0 != WNMemCmp(this, &_quaternion, sizeof(WNQuaternion)));
}

// Members /////////////////////////////////////////////////////////////////////////////////////////

WN_VOID WNQuaternion::Zero() {
    WNMemSet(&x, 0, sizeof(WNQuaternion));
}

WN_VOID WNQuaternion::Set(const WN_FLOAT32* _f) {
    WNMemCpy(&x, &_f[0], sizeof(WNQuaternion));
}

WN_VOID WNQuaternion::Set(WN_FLOAT32 _x, WN_FLOAT32 _y, WN_FLOAT32 _z, WN_FLOAT32 _w) {
    x = _x;
    y = _y;
    z = _z;
    w = _w;
}

WN_VOID WNQuaternion::Identity() {
    x = 0.0f;
    y = 0.0f;
    z = 0.0f;
    z = 1.0f;
}

WN_VOID WNQuaternion::Normalize() {
    const WN_FLOAT32 invLength = 1.0f / Length();

    *this *= invLength;
}

WN_VOID WNQuaternion::Invert() {
    *this = GetConjugate();
    this->Normalize();
}

WN_VOID WNQuaternion::Logarithm() {
    if (WNAbs(w) <= (1.0f - FLT_EPSILON)) {
        const WN_FLOAT32 theta = WNACos(w);
        const WN_FLOAT32 tdivSint  = theta / WNSin(theta);

        x *= tdivSint;
        y *= tdivSint;
        z *= tdivSint;
    }

    w = 0.0f;
}

WN_VOID WNQuaternion::Exponent() {
    const WNQuaternion quaternion = *this;
    const WN_FLOAT32 theta = WNSqrt(quaternion.x * quaternion.x +
                                    quaternion.y * quaternion.y +
                                    quaternion.z * quaternion.z);
    const WN_FLOAT32 cosTheta = WNCos(theta);

    if (WNAbs(theta) >= FLT_EPSILON) {
        const WN_FLOAT32 oneOverTheta = 1.0f / theta;
        const WN_FLOAT32 sinTheta = WNSin(theta);

        x = quaternion.x * oneOverTheta * sinTheta;
        y = quaternion.y * oneOverTheta * sinTheta;
        z = quaternion.z * oneOverTheta * sinTheta;
    }

    w = cosTheta;
}

WN_VOID WNQuaternion::Rotate(const WNQuaternion& _rotation) {
    *this *= _rotation;
}

WN_FLOAT32 WNQuaternion::Length() const {
    const WNQuaternion quaternion = *this;

    return(WNSqrt((quaternion.x * quaternion.x) + (quaternion.y * quaternion.y) +
                  (quaternion.z * quaternion.z) + (quaternion.w * quaternion.w)));
}

WN_FLOAT32 WNQuaternion::LengthSquared() const {
    const WNQuaternion quaternion = *this;

    return((quaternion.x * quaternion.x) + (quaternion.y * quaternion.y) +
           (quaternion.z * quaternion.z) + (quaternion.w * quaternion.w));
}

WN_BOOL WNQuaternion::IsZero() const {
    return(x == 0.0f && y == 0.0f && z == 0.0f && w == 0.0f);
}

WN_BOOL WNQuaternion::IsIdentity() const {
    return(x == 0.0f && y == 0.0f && z == 0.0f && w == 1.0f);
}

WNQuaternion WNQuaternion::GetNormalized() const {
    WNQuaternion quaternion = *this;

    return(quaternion.Normalize(), quaternion);
}

WNQuaternion WNQuaternion::GetInverted() const {
    WNQuaternion quaternion = *this;

    return(quaternion.Invert(), quaternion);
}

WNQuaternion WNQuaternion::GetConjugate() const {
    WNQuaternion quaternion = *this;

    quaternion.x = -quaternion.x;
    quaternion.y = -quaternion.y;
    quaternion.z = -quaternion.z;

    return(quaternion);
}

WNQuaternion WNQuaternion::GetLogarithm() const {
    WNQuaternion quaternion = *this;

    return(quaternion.Logarithm(), quaternion);
}

WNQuaternion WNQuaternion::GetExponent() const {
    WNQuaternion quaternion = *this;

    return(quaternion.Exponent(), quaternion);
}

WNMatrix44 WNQuaternion::ToMatrix44() const {
    const WN_FLOAT32 x2 = x + x;
    const WN_FLOAT32 y2 = y + y;
    const WN_FLOAT32 z2 = z + z;
    const WN_FLOAT32 wx = w * x2;
    const WN_FLOAT32 wy = w * y2;
    const WN_FLOAT32 wz = w * z2;
    const WN_FLOAT32 xx = x * x2;
    const WN_FLOAT32 yy = y * y2;
    const WN_FLOAT32 yz = y * z2;
    const WN_FLOAT32 xy = x * y2;
    const WN_FLOAT32 xz = x * z2;
    const WN_FLOAT32 zz = z * z2;

    return(WNMatrix44(1.0f - (yy + zz), xy - wz, xz + wy, 0.0f,
                      xy + wz, 1.0f - (xx + zz), yz - wx, 0.0f,
                      xz - wy, yz + wx, 1.0f - (xx + yy), 0.0f,
                      0.0f, 0.0f, 0.0f, 1.0f));
}